#ifndef CDYNAMICWINDOWAPPROACH_H
#define CDYNAMICWINDOWAPPROACH_H

#include "CoordinateTransformation.h"
#include <vector>

using namespace std;

#define INF                 ((2<<32)-1)
#define WEIGHT_HEADING      0.1						///< HEADING权重
#define WEIGHT_CLEARANCE    0.2                     ///< CLEARANCE权重
#define WEIGHT_VELOCITY     0.05					///< VELOCITY权重

/**
 * @brief 小艇建模后的物理量
 * @details 即动态窗口包含的状态量
 */
struct SParameterOfPhysicalConstraint
{
    double m_u;		///<横向速度    @details x方向，实际就是正东方向
    double m_v;		///<纵向速度    @details y方向，实际就是正北方向
    double m_psi;	///<艏向角      @details 以正北方向为正方向，顺时针方向计量
    double m_r;		///<角速度

    SParameterOfPhysicalConstraint(double u = 0, double v = 0, double psi = 0, double r = 0)
    {
        m_u = u;
        m_v = v;
        m_psi = psi;
        m_r = r;
    }
};
/**
 * @brief 小艇的各物理量约束
 * @details 速度、角速度、艏向角数值的上下限，用于确定动态窗口的解空间
 */
struct SLimitOfPhysicalConstraint
{
    double m_uMin;
    double m_uMax;
    double m_vMin;
    double m_vMax;
    double m_rMin;
    double m_rMax;
    double m_dUMax;         ///< 横向最大减速度
    double m_aUMax;         ///< 横向最大加速度
    double m_dVMax;         ///< 纵向最大减速度
    double m_aVMax;         ///< 纵向最大加速度
    double m_angularDMax;   ///< 最大角加速度
    double m_angularAMax;   ///< 最大角减速度
    double m_safetyDis;     ///< 安全距离，默认为5

    SLimitOfPhysicalConstraint( double uMax = 0, double vMax = 0,  double rMax = 0,
        double uMin = 0, double vMin = 0, double rMin = 0,
            double dUMax = 0, double aUMax = 0, double dVMax = 0, double aVMax = 0,
                double angularDMax = 0, double angularAMax = 0, double safetyDis = 5)
    {
        m_uMin = uMin;
        m_uMax = uMax;
        m_vMin = vMin;
        m_vMax = vMax;
        m_rMin = rMin;
        m_rMax = rMax;
        m_dUMax = dUMax;
        m_aUMax = aUMax;
        m_dVMax = dVMax;
        m_aVMax = aVMax;
        m_angularDMax = angularDMax;
        m_angularAMax = angularAMax;
        m_safetyDis = safetyDis;
    }
};

/**
 * @brief 动态窗口结构体
 * @details 包含ParameterOfPhysicalConstraint以及小艇的初始位置、与目标曲线的距离以及最近障碍物的距离
 * 目标坐标、障碍物坐标为世界坐标系，其中心点为小艇的出发点，为double型；
 */
struct SParameterOfWindow
{
    SParameterOfPhysicalConstraint m_phyParameter;
    double m_x;                      /**< x坐标                 @details 与输入目标、障碍物的坐标系保持一致，既可以是栅格坐标，也可以是世界坐标*/
    double m_y;                      /**< y坐标                 @details 与输入目标、障碍物的坐标系保持一致，既可以是栅格坐标，也可以是世界坐标*/
//    double m_distOfObstacle;    /**< 与最近障碍的距离       @details 与输入目标、障碍物的坐标系保持一致，既可以是栅格坐标，也可以是世界坐标*/
//    double m_distOfTarget;      /**< 与目标曲线的距离       @details 与输入目标、障碍物的坐标系保持一致，既可以是栅格坐标，也可以是世界坐标*/

    SParameterOfWindow(double initX, double initY, SParameterOfPhysicalConstraint phyParameter = SParameterOfPhysicalConstraint()
        /*double distOfObstacle = 0, double distOfTarget = INF*/)
    {
        m_x = initX;
        m_y = initY;
        m_phyParameter = phyParameter;
//        m_distOfObstacle = distOfObstacle;
//        m_distOfTarget = distOfTarget;
    }
};

/**
 * @brief 经纬度结构体
 * @details 描述经纬度，默认值为宜昌市的经纬度，该经纬度数据为bd09坐标系下的数值
 */
struct SWaypoint
{
    double m_lng;	///< 经度
    double m_lat;	///< 纬度

    /**
     * @brief 初始化经纬度，默认值为宜昌市的经纬度
     * @param lng   经度
     * @param lat   纬度
     */
    SWaypoint(double lng = 111.293718, double lat = 30.695929)
    {
        m_lng = lng;
        m_lat = lat;
    }
};

/**
 * @brief 动态窗口法类
 * @details 实现动态窗口算法
 */
class CDynamicWindowApproach
{
public:

    /**
     * @brief 评估函数结构体
     * @details f = sigma(alpha*heading + beta*clearance + gamma*velocity)
     */
    struct SEvaluationParameter
    {
        double m_heading, m_clearance, m_velocity, m_u, m_v, m_r;
        /**
         * @brief 默认构造函数
         * @param heading   @details DWA原算法中表示与target的朝向，在本算法中表示与目标航线之间的距离
         * @param clearance @details 表示与最近障碍物之间的距离，要注意必须大于等于安全距离（也就是刹车距离）
         * @param velocity  @details 原DWA算法中表示速度，在本算法中由于速度已经被分解为u和v，所以应当是sqrt(pow(u,2)+pow(v,2))
         * @param u 横向速度
         * @param v 纵向速度
         * @param r 角速度
         */
        SEvaluationParameter(double heading = 0, double clearance = 0, double velocity = 0,
                             double u = 0, double v = 0, double r = 0)
        {
            m_heading = heading;
            m_clearance = clearance;
            m_velocity = velocity;
            m_u = u;
            m_v = v;
            m_r = r;
        }
    };

    CDynamicWindowApproach(const SWaypoint &goal, const SWaypoint &startPos, SParameterOfPhysicalConstraint *phyParameter = nullptr, SLimitOfPhysicalConstraint *limit = nullptr,
                           int wcount = 30, double samplingRate = 0.1, double samplingVelocity = 0.01, double samplingAngularVelocity = PI/180)
    {
        m_goal = goal;
        m_startPos = startPos;
        m_phyParameter = phyParameter;
        m_limit = limit;
        m_wcount = wcount;
        m_samplingRate = samplingRate;
        m_samplingVelocity = samplingVelocity;
        m_samplingAngularVelocity = samplingAngularVelocity;
        m_x = 0;
        m_y = 0;
    }
    CDynamicWindowApproach(const double &lngOfGoal, const double &latOfGoal, const double &lngOfStart, const double &latOfStart, SParameterOfPhysicalConstraint *phyParameter = nullptr, SLimitOfPhysicalConstraint *limit = nullptr,
                           int wcount = 30, double samplingRate = 0.1, double samplingVelocity = 0.01, double samplingAngularVelocity = PI/180)
    {
        m_goal = SWaypoint(lngOfGoal, latOfGoal);
        m_startPos = SWaypoint(lngOfStart, latOfStart);
        m_phyParameter = phyParameter;
        m_limit = limit;
        m_wcount = wcount;
        m_samplingRate = samplingRate;
        m_samplingVelocity = samplingVelocity;
        m_samplingAngularVelocity = samplingAngularVelocity;
        m_x = 0;
        m_y = 0;
    }
    ~CDynamicWindowApproach(void)
    {
        m_phyParameter = nullptr;
        m_limit = nullptr;
    }

    vector<SParameterOfWindow> PlanPath(vector<pair<double, double>> &waypoints);
    vector<double> DynamicWindowApproch(SParameterOfWindow currentState, vector<pair<double, double>> &obstacles, vector<pair<double, double>> &waypoints);
    vector<double> CreateSolutionSpace(SParameterOfPhysicalConstraint *phyParameter);
    SParameterOfWindow CalculateNextWindowParameter(const SParameterOfWindow currentWindow, const double u, const double v, const double r/*, const vector<pair<double, double> > &obstacles, const vector<pair<double, double> > &waypoints*/);
    double CalculateDisOfObstacle(double x, double y, const vector<pair<double, double>> &obstacles);
    double CalculateDisOfTarget(double x, double y, const vector<pair<double, double>> &waypoints);
    vector<SParameterOfWindow> GenerateTraj(const SParameterOfWindow &currentWindow, const double u, const double v, const double r/*, const vector<pair<double, double> > &obstacles, const vector<pair<double, double> > &waypoints*/);
    double CalculateBreakingDist(double u, double v);

    SParameterOfPhysicalConstraint *m_phyParameter;		///< 状态变量
    SLimitOfPhysicalConstraint *m_limit;				///< 状态上下限
    SWaypoint m_goal;                                   ///< 终点的经纬度
    SWaypoint m_startPos;                               ///< 起点的经纬度
    double m_x;                                         ///< 当前x方向坐标    @details 基于世界坐标系
    double m_y;                                         ///< 当前y方向坐标    @details 基于世界坐标系
    int m_wcount;										///< 一次规划的窗口数
    double m_samplingRate;                              ///< 采样时间间隔     @details 在该时间内，认为小艇处于匀速直线运动, m_wcount*m_samplingRate=预测时间，默认预测未来3s时间的轨迹
    double m_samplingVelocity;                          ///< 速度采样增量     @details 默认0.01（u、v均适用）
    double m_samplingAngularVelocity;                   ///< 角速度采样增量   @details 默认1°（pi/180）
};

#endif // CDYNAMICWINDOWAPPROACH_H
